#pragma once
#ifndef HGRAPH_PLANNER_H
#define HGRAPH_PLANNER_H

#include "basic_typedef.h"
#include "Graph.h"
#include "MultiRobotCollisionDetector.h"
#include "stdlib.h"
#include "math.h"
#include "MultiRobotPRM.h"
#include "MultiRobotClearanceDetector.h"

class HGraphPlanner
{
public:
    HGraphPlanner(vector<Polygon_2> &robot_polygons, int robot_num, Obstacles &obstacles, vector<Conf> &start_confs, vector<Conf> &target_confs, Room &room, bool measure, double alpha);

    void generate_roadmaps(int num_of_paths, int number_vertices, int k_nearest, double eps = 1);
    double update_path(vector<vector<Conf>> &path);

private:
    enum MOVEMENT{
        MATCH,
        UP,
        LEFT
    };

    static const int GAP_EXT_PRICE;
    static const int MAX_VALUE;

    double getDelta(Point_d p, Point_d q);
    vector<vector<Point_d>> getPotentialBridgeEdges(vector<Point_d> p, vector<Point_d> q);
    double getWeight(Point_d p, Point_d q);
    void calculateDynamicPathMatching(vector<Point_d> p, vector<Point_d> q);
    void printMatrices(int m, int n);
    double measure(list<Point_d>& path);

    vector<Polygon_2>           &m_robot_polygons;
    int                         m_robot_num;
    Obstacles                   &m_obstacles;
    vector<Conf>                &m_start_confs, &m_target_confs;
    Room                        &m_room;
    bool                        m_measure;
    double                      m_alpha;
    
    vector<MPATH>               m_paths;
    Graph<Point_d, cmpPoint_d>  m_hgraph;                // Graph structure
    map<Point_d, vector<Conf>, cmpPoint_d>  m_map;  //Maps between a pointer to point_d to point in the configuration space
    MultiRobotCollisionDetector m_mr_collision_detector;
    map<pair<int,int>,int>      m_costMatrix;
    map<pair<int,int>,MOVEMENT> m_traceBackMatrix;
    MultiRobotClearanceDetector m_detector;
};

#endif